#!/usr/bin/env pybricks-micropython
from MyRobot import MyRobot
from MyScreen import MyScreen
 
import time
from pybricks.ev3devices import InfraredSensor
from pybricks.parameters import Port,Button
from pybricks.hubs import EV3Brick
from pybricks.tools import wait, StopWatch
from pybricks.media.ev3dev import Font

ev3 = EV3Brick()
myInfrared = InfraredSensor(Port.S3)

myGameStart= True
myscreen = MyScreen(ev3)

#主界面显示，选择速度和方式
while (myscreen.confirmGo ==0):  
    wait(200)
    myscreen.showScreen()
    myscreen.monitorButton()

ev3.speaker.play_notes(['F4/4','F3/4'])  

print("start the task")
print("speedOption:" + str(myscreen.speedOption))
print("wayOption:" + str(myscreen.wayOption))

mybot = MyRobot(ev3,myscreen.wayOption,myscreen.speedOption)

mybot.addLog("start speed:"+ str(myscreen.speedOption) + " way:"+ str(myscreen.wayOption))
# mybot.goTask2_Group1()


#开始运行
while myGameStart:
    button1=myInfrared.buttons(1)
    button2=myInfrared.buttons(2)
    button3=myInfrared.buttons(3)
    button4=myInfrared.buttons(4)
   #  print (mybot.ultrasonic.distance())
   #  print(mybot.colorChecker.color())
    #debug mode
    if len(button1)>0 :
        if len(button1)==1:
           #start task1
           if (Button.LEFT_UP in button1):
              print("Main Menu1: LEFT_UP is pressed")
              mybot.goTask1()

           #start task2
           if (Button.LEFT_DOWN in button1):
              print("Main Menu1: LEFT_DOWN is pressed")
              mybot.goTask2()


           
           if (Button.RIGHT_UP in button1):
              print("Main Menu1: RIGHT_UP is pressed")  
           
           if (Button.RIGHT_DOWN in button1):
              print("Main Menu1: RIGHT_DOWN is pressed")  
           
           if (Button.BEACON in button1):
              print("Main Menu1: BEACON is pressed")  
      
    elif len(button2)>0:
        if len(button2)==1:
         
         
           if (Button.LEFT_UP in button2):
              print("Main Menu4: LEFT_UP is pressed")
              mybot.goTask2_Group1()
          
          
          
           if (Button.RIGHT_UP in button2):
              print("Main Menu4: RIGHT_UP is pressed")  
              mybot.goTask2_Group2()
             # mybot.goTask1_Group3()
          
          
           if (Button.LEFT_DOWN in button2):
              print("Main Menu4: LEFT_DOWN is pressed")
              mybot.goTask1_Group3()

  
  
           if (Button.RIGHT_DOWN in button2):
              print("Main Menu4: RIGHT_DOWN is pressed")  
            #   mybot.goTask1_Group4()
     
     
           if (Button.BEACON in button2):
              print("Main Menu4: BEACON is pressed")  
          
          

    elif len(button3)>0:
         if len(button3)==1:
            #robot zhuaGan up
            if (Button.LEFT_UP in button3):
               print("Main Menu3: LEFT_UP is pressed")
               mybot.zhuaGanUpByDegree(280,90)
               while Button.LEFT_UP in mybot.ev3.buttons.pressed():
                  wait(10)
            #robot zhuaGan down
            if (Button.LEFT_DOWN in button3):
               print("Main Menu3: LEFT_DOWN is pressed")
               mybot.zhuaGanDownByDegree(280,90)

            #robot zhuaZi open
            if (Button.RIGHT_UP in button3):
               print("Main Menu3: RIGHT_UP is pressed")   
               mybot.zhuaZiOpen(360,90)

            #robot zhuaZi close
            if (Button.RIGHT_DOWN in button3):
               print("Main Menu3: RIGHT_DOWN is pressed")
               mybot.zhuaZiClose(360,90)
                    
         elif len(button3)==2:
          
           button3temp=myInfrared.buttons(3)
           if len(button3temp) ==2:
              if (button3temp[0] in button3) &  (button3temp[1] in button3):
                  #robot go ahead
                  if (Button.LEFT_UP in button3) & (Button.RIGHT_UP in button3):
                     print("Main Menu3: LEFT_UP&RIGHT_UP are pressed")
                     mybot.goAhead(15)

                  #robot go back
                  if (Button.LEFT_DOWN in button3) & (Button.RIGHT_DOWN in button3):
                     print("Main Menu3: LEFT_DOWN&RIGHT_DOWN are pressed")
                     mybot.goAhead(-15)

                  #robot turn left
                  if (Button.LEFT_UP in button3) & (Button.RIGHT_DOWN in button3):
                     print("Main Menu3: LEFT_DOWN&RIGHT_DOWN are pressed")
                     mybot.turn(10)

                  #robot turn right
                  if (Button.LEFT_DOWN in button3) & (Button.RIGHT_UP in button3):
                     print("Main Menu3: LEFT_DOWN&RIGHT_UP are pressed")
                     mybot.turn(-10)

                  #robot confirm zhuaZi Position
                  if (Button.LEFT_DOWN in button3) & (Button.LEFT_UP in button3):
                     print("Main Menu3: LEFT_UP&LEFT_DOWN are pressed")
                     mybot.zhuaZiInitalPostion()
    
    elif len(button4) > 0:
        if len(button4)==1:
           #robot zhuaGan up
           if (Button.LEFT_UP in button4):
               mybot.goTask2_Group1()
           if (Button.LEFT_DOWN in button4):
               mybot.goTask2_Group3()
           if (Button.RIGHT_UP in button4):
               print("Main Menu3: RIGHT_UP is pressed")
               mybot.goTask2_Group2()
           if (Button.RIGHT_DOWN in button4):
               print("Main Menu3: RIGHT_DOWN is pressed")
               mybot.goTask2_Group4()

 